#include <mml/primitives.h>

#include <mml/api.h>
#include <mml/util.h>

#include <iostream>
#include <iomanip>
#include <math.h>

/** Maximum element. */
template <typename Type>
Type const& mml::vector_max(int n, Type const* x)
{
  int maxindex = 0;
  for (int i = 1; i < n; ++i) {
    if (x[i] > x[maxindex]) {
      maxindex = i;
    }
  }
  return x[maxindex];
}
template MMLAPI double const& mml::vector_max<double>(int n, double const* x);
template MMLAPI float const& mml::vector_max<float>(int n, float const* x);



/** Minimum element. */
template <typename Type>
Type const& mml::vector_min(int n, Type const* x)
{
  int minindex = 0;
  for (int i = 1; i < n; ++i) {
    if (x[i] < x[minindex]) {
      minindex = i;
    }
  }
  return x[minindex];
}
template MMLAPI double const& mml::vector_min<double>(int n, double const* x);
template MMLAPI float const& mml::vector_min<float>(int n, float const* x);



/** Vector cross product. */
template <typename Type>
void mml::vector_cross(Type const* a, Type const* b, Type* c)
{
  c[0] = a[1] * b[2] - a[2] * b[1];
  c[1] = a[2] * b[0] - a[0] * b[2];
  c[2] = a[0] * b[1] - a[1] * b[0];
}
template MMLAPI void mml::vector_cross<double>(double const* a, double const* b, double* c);
template MMLAPI void mml::vector_cross<float>(float const* a, float const* b, float* c);



/** Print a vector. */
template <typename Type>
void mml::vector_print(int n, Type const* data, std::ostream& s)
{
  s << "(";
  if (n > 0) {
    for (int i = 0; i < n - 1; ++i) {
      s << data[i] << ", ";
    }
    s << data[n - 1];
  }
  s << ")";
}
template MMLAPI void mml::vector_print<double>(int n, double const* data, std::ostream& s);
template MMLAPI void mml::vector_print<float>(int n, float const* data, std::ostream& s);



/** Read a vector. */
template <typename Type>
void mml::vector_read(int n, Type* data, std::istream& s)
{
  char dummy;

  // read away '(':
  do {
    s >> dummy;
  } while (dummy != '(' && !s.eof());

  if (n > 0) {
    s >> data[0];  // read first element
    for (int i = 1; i < n; ++i) {
      // read away ', ':
      do {
        s >> dummy;
      } while (dummy != ',' && !s.eof());
      s >> data[i];
    }
  }

  // read away ')':
  do {
    s >> dummy;
  } while (dummy != ')' && !s.eof());
}
template MMLAPI void mml::vector_read<double>(int n, double* data, std::istream& s);
template MMLAPI void mml::vector_read<float>(int n, float* data, std::istream& s);



/** Copy a vector */
template <typename Type>
void mml::vector_copy(int n, Type* target, Type const* source)
{
  for (int i = 0; i < n; ++i) {
    target[i] = source[i];
  }
}
template MMLAPI void mml::vector_copy<double>(int n, double* target, double const* source);
template MMLAPI void mml::vector_copy<float>(int n, float* target, float const* source);



/** The infinity norm (magnitude of largest component). */
template <typename Type>
Type mml::vector_norminf(int n, Type const* x)
{
  Type result(0.0f);
  for (int i = 0; i < n; ++i) {
    Type candidate = fabs(x[i]);
    if (candidate > result) {
      result = candidate;
    }
  }
  return result;
}
template MMLAPI double mml::vector_norminf<double>(int n, double const* x);
template MMLAPI float mml::vector_norminf<float>(int n, float const* x);



/** The squared 2-norm of a vector. */
template <typename Type>
Type mml::vector_normsq(int n, Type const* x)
{
  Type result(0.0f);
  for (int i = 0; i < n; ++i) {
    result += x[i] * x[i];
  }
  return result;
}
template MMLAPI double mml::vector_normsq<double>(int n, double const* x);
template MMLAPI float mml::vector_normsq<float>(int n, float const* x);



/** The 2-norm of a vector. */
template <typename Type>
Type mml::vector_norm(int n, Type const* x)
{
  return sqrt(vector_normsq(n, x));
}
template MMLAPI double mml::vector_norm<double>(int n, double const* x);
template MMLAPI float mml::vector_norm<float>(int n, float const* x);



/** Angle between two vectors. */
template <typename Type>
Type mml::vector_angle(int n, Type const* a, Type const* b)
{
  return atan2(sqrt(vector_abmabsq(n, a, b)), vector_ip(n, a, b));
}
template MMLAPI double mml::vector_angle<double>(int n, double const* a, double const* b);
template MMLAPI float mml::vector_angle<float>(int n, float const* a, float const* b);



/** Normal to a vector. */
template <typename Type>
void mml::vector_normal(int n, Type const* x, Type* y)
{
  // Zero target vector:
  vector_reset(n, y);

  // Find index of the two largest elements (in an absolute sense):
  int maxIndex = -1;
  int subMaxIndex = -1;
  Type maxVal = Type(0.0f);
  Type subMaxVal = Type(0.0f);
  for (int i = 0; i < n; ++i) {
    Type candidate = fabs(x[i]);
    if (candidate > maxVal) {
      subMaxIndex = maxIndex;
      subMaxVal = maxVal;
      maxIndex = i;
      maxVal = candidate;
    } else if (candidate > subMaxVal) {
      subMaxIndex = i;
      subMaxVal = candidate;
    }
  }

  if (subMaxIndex == -1) {
    if (maxIndex != -1) {
      // Vector with only one nonzero component, put it one index further:
      y[(maxIndex + 1) % n] = x[maxIndex];
    }
  } else {
    // Perform a 90 degree Givens rotation over the largest components:
    y[subMaxIndex] = -x[maxIndex];
    y[maxIndex] = x[subMaxIndex];

    // Orthogonalize (subtract projection):
    vector_add(n, y, x, -(y[subMaxIndex] * x[subMaxIndex] + y[maxIndex] * x[maxIndex]) / vector_normsq(n, x));
  }

  // Normalize the result:
  vector_normalize(n, y);
}
template MMLAPI void mml::vector_normal<double>(int n, double const* x, double* y);
template MMLAPI void mml::vector_normal<float>(int n, float const* x, float* y);



/** Interchange contents of two (non-overlapping) vectors. */
template <typename Type>
void mml::vector_swap(int n, Type* x, Type* y)
{
  for (int i = 0; i < n; ++i) {
    Type temp = x[i];
    x[i] = y[i];
    y[i] = temp;
  }
}
template MMLAPI void mml::vector_swap<double>(int n, double* x, double* y);
template MMLAPI void mml::vector_swap<float>(int n, float* x, float* y);



/** Normalize a vector. */
template <typename Type>
void mml::vector_normalize(int n, Type* x)
{
  Type nrm = vector_norm(n, x);
#ifdef _DEBUG
  if (nrm == Type(0.0f)) {
    throw mml::DivisionByZero();
  }
#endif
  vector_scale(n, x, Type(1.0f) / nrm);
}
template MMLAPI void mml::vector_normalize<double>(int n, double* x);
template MMLAPI void mml::vector_normalize<float>(int n, float* x);



/** Inner product of two vectors. */
template <typename Type>
Type mml::vector_ip(int n, Type const* x, Type const* y)
{
  Type ret(0.0f);
  for (int i = 0; i < n; ++i) {
    ret += x[i] * y[i];
  }
  return ret;
}
template MMLAPI double mml::vector_ip<double>(int n, double const* x, double const* y);
template MMLAPI float mml::vector_ip<float>(int n, float const* x, float const* y);



/** Scale a vector. */
template <typename Type>
void mml::vector_scale(int n, Type* x, Type const& scale)
{
  for (int i = 0; i < n; ++i) {
    x[i] *= scale;
  }
}
template MMLAPI void mml::vector_scale<double>(int n, double* x, double const& scale);
template MMLAPI void mml::vector_scale<float>(int n, float* x, float const& scale);



/** Add a scaled vector to another. */
template <typename Type>
void mml::vector_add(int n, Type* x, Type const* y, Type const& scale)
{
  for (int i = 0; i < n; ++i) {
    x[i] += scale * y[i];
  }
}
template MMLAPI void mml::vector_add<double>(int n, double* x, double const* y, double const& scale);
template MMLAPI void mml::vector_add<float>(int n, float* x, float const* y, float const& scale);



/** Add a vector to another. */
template <typename Type>
void mml::vector_add(int n, Type* x, Type const* y)
{
  for (int i = 0; i < n; ++i) {
    x[i] += y[i];
  }
}
template MMLAPI void mml::vector_add<double>(int n, double* x, double const* y);
template MMLAPI void mml::vector_add<float>(int n, float* x, float const* y);



/** Subtract a vector from another. */
template <typename Type>
void mml::vector_subtract(int n, Type* x, Type const* y)
{
  for (int i = 0; i < n; ++i) {
    x[i] -= y[i];
  }
}
template MMLAPI void mml::vector_subtract<double>(int n, double* x, double const* y);
template MMLAPI void mml::vector_subtract<float>(int n, float* x, float const* y);



/** Negate a vector. */
template <typename Type>
void mml::vector_negate(int n, Type* x)
{
  for (int i = 0; i < n; ++i) {
    x[i] = -x[i];
  }
}
template MMLAPI void mml::vector_negate<double>(int n, double* x);
template MMLAPI void mml::vector_negate<float>(int n, float* x);



/** Set a vector to a value. */
template <typename Type>
void mml::vector_set(int n, Type* x, Type const& s)
{
  for (int i = 0; i < n; ++i) {
    x[i] = s;
  }
}
template MMLAPI void mml::vector_set<double>(int n, double* x, double const& s);
template MMLAPI void mml::vector_set<float>(int n, float* x, float const& s);



/** Reset a vector to default value. */
template <typename Type>
void mml::vector_reset(int n, Type* x)
{
  vector_set(n, x, Type());
}
template MMLAPI void mml::vector_reset<double>(int n, double* x);
template MMLAPI void mml::vector_reset<float>(int n, float* x);



// Calculate the difference ||a||^2 * ||b||^2 - (a * b)^2 in a numerically stable way.
template <typename Type>
Type mml::vector_abmabsq(int n, Type const* a, Type const* b)
{
  Type globalsum = Type(0.0f);
  for (int i = 0; i < n; ++i) {
    Type localsum0 = 0.0f;
    Type localsum1 = 0.0f;
    // Skip the terms for which j == i:
    for (int j = 0; j < i; ++j) {
      localsum0 += b[j] * b[j];
      localsum1 += a[j] * b[j];
    }
    for (int j = i + 1; j < n; ++j) {
      localsum0 += b[j] * b[j];
      localsum1 += a[j] * b[j];
    }
    globalsum += a[i] * a[i] * localsum0 - a[i] * b[i] * localsum1;
  }

  return globalsum;
}
template MMLAPI double mml::vector_abmabsq<double>(int n, double const* a, double const* b);
template MMLAPI float mml::vector_abmabsq<float>(int n, float const* a, float const* b);



// Calculate the difference ||a|| ||b|| - a * b in a numerically stable way
template <typename Type>
Type mml::vector_abmab(int n, Type const* a, Type const* b)
{
  Type ip = vector_ip(n, a, b);  // inner product
  Type normproduct = vector_norm(n, a) * vector_norm(n, b);
  Type result;
  if (ip > Type(0.0f)) {
    // Special form that reduces round-off error:
    result = vector_abmabsq(n, a, b) / (normproduct + ip);
  } else {
    // normal operation
    result = normproduct - ip;
  }

  return result;
}
template MMLAPI double mml::vector_abmab<double>(int n, double const* a, double const* b);
template MMLAPI float mml::vector_abmab<float>(int n, float const* a, float const* b);



/** Vector outer product. */
template <typename Type>
void mml::vector_outerProduct(int m, int n, Type const* a, Type const* b, Type* A)
{
  for (int i = 0; i < m; ++i) {
    for (int j = 0; j < n; ++j) {
      A[n * i + j] = a[i] * b[j];
    }
  }
}
template MMLAPI void mml::vector_outerProduct<double>(int m, int n, double const* a, double const* b, double* A);
template MMLAPI void mml::vector_outerProduct<float>(int m, int n, float const* a, float const* b, float* A);



/** Grassmann product of two quaternions. */
template <typename Type>
void mml::quat_grassmann(Type const* a, Type const* b, Type* c)
{
  c[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
  c[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
  c[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
  c[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
}
template MMLAPI void mml::quat_grassmann<double>(double const* a, double const* b, double* c);
template MMLAPI void mml::quat_grassmann<float>(float const* a, float const* b, float* c);



/** Print a matrix. */
template <typename Type>
void mml::matrix_print(int m, int n, Type const* A, std::ostream& s)
{
  for (int i = 0; i < m; ++i) {
    for (int j = 0; j < n; ++j) {
      s << std::setw(15) << A[i * n + j];
    }
    s << std::endl;
  }
}
template MMLAPI void mml::matrix_print<double>(int m, int n, double const* A, std::ostream& s);
template MMLAPI void mml::matrix_print<float>(int m, int n, float const* A, std::ostream& s);



/** Generate the transpose of a matrix. */
template <typename Type>
void mml::matrix_transpose(int m, int n, Type const* A, Type* At)
{
  for (int i = 0; i < m; ++i) {  // for all rows of A
    for (int j = 0; j < n; ++j) {  // for all columns of A
      At[m * j + i] = A[n * i + j];  // copy element
    }
  }
}
template MMLAPI void mml::matrix_transpose<double>(int m, int n, double const* A, double* At);
template MMLAPI void mml::matrix_transpose<float>(int m, int n, float const* A, float* At);



/** Multiply a vector by a matrix b = Ax. */
template <typename Type>
void mml::matrix_matvecmul(int m, int n, Type const* A, Type const* x, Type* b)
{
  for (int i = 0; i < m; ++i) {
    b[i] = vector_ip(n, &A[n * i], x);
  }
}
template MMLAPI void mml::matrix_matvecmul<double>(int m, int n, double const* A, double const* x, double* b);
template MMLAPI void mml::matrix_matvecmul<float>(int m, int n, float const* A, float const* x, float* b);



/** Multiply a matrix by a matrix C = A * B. */
template <typename Type>
void mml::matrix_matmatmul(int m, int n, int o, Type const* A, Type const* B, Type* C)
{
  vector_reset(m * o, C);  // reset C
  for (int i = 0; i < m; ++i) {
    for (int j = 0; j < n; ++j) {
      vector_add(o, &C[o * i], &B[o * j], A[n * i + j]);
    }
  }
}
template MMLAPI void mml::matrix_matmatmul<double>(int m, int n, int o, double const* A, double const* B, double* C);
template MMLAPI void mml::matrix_matmatmul<float>(int m, int n, int o, float const* A, float const* B, float* C);



/** Reset a matrix. */
template <typename Type>
void mml::matrix_reset(int m, int n, Type* matrix)
{
  vector_reset(m * n, matrix);
}
template MMLAPI void mml::matrix_reset<double>(int m, int n, double* matrix);
template MMLAPI void mml::matrix_reset<float>(int m, int n, float* matrix);



/** Add a matrix to another. */
template <typename Type>
void mml::matrix_add(int m, int n, Type* target, Type const* source)
{
  vector_add(m * n, target, source);
}
template MMLAPI void mml::matrix_add<double>(int m, int n, double* target, double const* source);
template MMLAPI void mml::matrix_add<float>(int m, int n, float* target, float const* source);
